 

/******************************************************************************
 * Includes
 *****************************************************************************/
#include "virtual_com.h"
#include <i2c.h>

extern uint_16		*EMG_pwm0;
extern uint_16		*EMG_pwm1;
extern uint_16		*EMG_pwm2;
extern uint_16		*EMG_pwm3;


extern void Main_Task(uint_32 param);

/*****************************************************************************
 * Constant and Macro's 
 *****************************************************************************/
#define MAIN_TASK       10

#define PACKET_IN_QUEUE   255
#define ROBOT_SERIAL_MODE 0		// 0 = USB CDC, 1 = SERIAL
#define DTE_SUPPORT	  	  0		// 0 = Disable DTE support
#define MMA7455			  1
#define MMA7455_10BIT	  1		// 0 = 8bit mode, 1 = 10bit mode
#define MY_ADC "adc:" 

TASK_TEMPLATE_STRUCT  MQX_template_list[] =
{
   { MAIN_TASK, Main_Task, 2*3000L, 7L, "Main", MQX_AUTO_START_TASK, 0, 0},
   { 0L, 0L, 0L, 0L, 0L, 0L, 0, 0}
};

const ADC_INIT_CHANNEL_STRUCT adc_channel_param1 = 
{
    6, /* physical ADC channel */
    ADC_CHANNEL_MEASURE_LOOP | ADC_CHANNEL_START_NOW, /* runs continuously after IOCTL trigger */
    10,             /* number of samples in one run sequence */
    0,              /* time offset from trigger point in us */
    300000,         /* period in us (= 0.6 sec) */
    0x10000,        /* scale range of result (not used now) */
    10,             /* circular buffer size (sample count) */
    0,     			/* logical trigger ID that starts this ADC channel */
#if MQX_USE_LWEVENTS
    NULL,
    0x01            /* mask of event to be set */
#endif
};

/* ADC device init struct */
const ADC_INIT_STRUCT adc_init = {
    ADC_RESOLUTION_DEFAULT,     /* resolution */
};

#define I2C_ID		0x5A
#define INIT_MPR121(r,d) i2c_write_byte( g_fd, I2C_ID, r, d ); 


/*****************************************************************************
 * Global Functions Prototypes
 *****************************************************************************/
void USBRobot(void);

/****************************************************************************
 * Global Variables
 ****************************************************************************/              
extern USB_ENDPOINTS usb_desc_ep;
extern DESC_CALLBACK_FUNCTIONS_STRUCT  desc_callback;

CDC_HANDLE   g_app_handle;

uint_8 packet_in_queue[PACKET_IN_QUEUE];
uint_8 packet_in_queue_in_index;
uint_8 packet_in_queue_out_index;

/*****************************************************************************
 * Local Types - None
 *****************************************************************************/
 
/*****************************************************************************
 * Local Functions Prototypes
 *****************************************************************************/
void USB_App_Callback(uint_8 event_type, void* val,pointer arg);
void USB_Notif_Callback(uint_8 event_type,void* val,pointer arg); 
void Virtual_Com_App(void);
/*****************************************************************************
 * Local Variables 
 *****************************************************************************/
static boolean start_app = FALSE;
static boolean start_transactions = FALSE;
static uint_8 *g_curr_recv_buf;
static uint_8 *g_curr_send_buf;
static uint_8 g_recv_size;
static uint_8 g_send_size;
static uint_8 *g_packet;
static uint_8 *g_ReturnData;
static uint_8 g_packet_index;
static FILE_PTR g_fd, g_fdadc0;
static uint_8 g_boot_message;


/*****************************************************************************
 * Local Functions
 *****************************************************************************/


// START ROBOT ------------------------------------------------------------------------------------------------
// START ROBOT ------------------------------------------------------------------------------------------------
// START ROBOT ------------------------------------------------------------------------------------------------
// START ROBOT ------------------------------------------------------------------------------------------------


/*FUNCTION****************************************************************
* 
* Function Name    : i2c_write_register
* Returned Value   : void
* Comments         : 
*   Writes the provided data buffer at address in I2C buffer
*
*END*********************************************************************/

void i2c_write_register
   (
      /* [IN] The file pointer for the I2C channel */
      FILE_PTR   fd,

      /* [IN] */
      uint_32    device_addr,

      /* [IN] The address in EEPROM to write to */
      uint_32    addr,

      /* [IN] The array of characters are to be written in EEPROM */
      uchar_ptr  buffer,

      /* [IN] Number of bytes in that buffer */
      uint_32    n      
   )
{ /* Body */

   uint_32       param;
   uint_32       result;
   uint_8        mem;

   printf ("Writing %d bytes to address 0x%08x ...\n", n, addr);
      /* I2C bus address also contains memory block index */
      param = device_addr;
      printf ("Set I2C bus address to 0x%02x ... ", param);
      if (I2C_OK == ioctl (fd, IO_IOCTL_I2C_SET_DESTINATION_ADDRESS, &param))
      {
         printf ("OK\n");
      } else {
         printf ("ERROR\n");
      }

      /* Initiate start and send I2C bus address */
      printf ("I2C start, send address and get ack ... ");
      fwrite (&mem, 1, 0, fd);

      /* Check ack (device exists) */
      if (I2C_OK == ioctl (fd, IO_IOCTL_FLUSH_OUTPUT, &param))
      {
         printf ("OK ... ack == %d\n", param);
         if (param) 
         {
            /* Stop I2C transfer */
            printf ("Stop transfer ... ");
            if (I2C_OK == ioctl (fd, IO_IOCTL_I2C_STOP, NULL))
            {
               printf ("OK\n");
            } else {
               printf ("ERROR\n");
            }
            return;
         }
      } else {
         printf ("ERROR\n");
      }

      /* Write address within memory block */
      mem = addr & 0xFF;
      printf ("Write to address 0x%02x ... ", mem);
      result = fwrite (&mem, 1, 1, fd);
      if (1 == result)
      {
         printf ("OK\n");
      } else {
         printf ("ERROR\n");
      }

      /* Page write of data */
      printf ("Page write %d bytes ... ", n);
      result = fwrite (buffer, 1, n, fd);
      if (result == n )
      {
         printf ("OK\n");
      } else {
         printf ("ERROR\n");
      }
      
      /* Wait for completion */
      printf ("Flush ... ");
      result = fflush (fd);
      if (MQX_OK == result)
      {
         printf ("OK\n");
      } else {
         printf ("ERROR\n");
      }

      /* Stop I2C transfer - initiate EEPROM write cycle */
      printf ("Stop transfer ... ");
      if (I2C_OK == ioctl (fd, IO_IOCTL_I2C_STOP, NULL))
      {
         printf ("OK\n");
      } else {
         printf ("ERROR\n");
      }
} /* Endbody */


void i2c_write_byte( FILE_PTR   fd, uint_8 device_addr, uint_8 r, uint_8 d )
{
	uint_8  temp;
	
	temp = d;
	i2c_write_register( fd, device_addr, r, &temp, 1 );
	_time_delay (2L);
}
   
/*FUNCTION****************************************************************
* 
* Function Name    : i2c_read_register
* Returned Value   : void
* Comments         : 
*   Reads into the provided data buffer from address in I2C register
*
*END*********************************************************************/

void i2c_read_register
   (
      /* [IN] The file pointer for the I2C channel */
      FILE_PTR   fd,

      /* [IN] */
      uint_32    device_addr,

      /* [IN] The address in EEPROM to read from */
      uint_32    addr,

      /* [IN] The array of characters to be written into */
      uchar_ptr  buffer,

      /* [IN] Number of bytes to read */
      uint_32    n      
   )
{ /* Body */
   uint_32       param;
   uint_32       result;
   uint_8        mem;

   printf ("Reading %d bytes from address 0x%08x ...\n", n, addr);
   if (0 == n) return;

   /* I2C bus address also contains memory block index */
   param = device_addr;
   printf ("Set I2C bus address to 0x%02x ... ", param);
   if (I2C_OK == ioctl (fd, IO_IOCTL_I2C_SET_DESTINATION_ADDRESS, &param))
   {
      printf ("OK\n");
   } else {
      printf ("ERROR\n");
   }

   /* Initiate start and send I2C bus address */
   printf ("I2C start, send address and get ack ... ");
   fwrite (&mem, 1, 0, fd);

   /* Check ack (device exists) */
   if (I2C_OK == ioctl (fd, IO_IOCTL_FLUSH_OUTPUT, &param))
   {
      printf ("OK ... ack == %d\n", param);
      if (param) 
      {
         /* Stop I2C transfer */
         printf ("Stop transfer ... ");
         if (I2C_OK == ioctl (fd, IO_IOCTL_I2C_STOP, NULL))
         {
            printf ("OK\n");
         } else {
            printf ("ERROR\n");
         }
         return;
      }
   } else {
      printf ("ERROR\n");
   }

   /* Write address within memory block */
   mem = addr & 0xFF;
   printf ("Write address 0x%02x to read from ... ", mem);
   result = fwrite (&mem, 1, 1, fd);
   if (1 == result)
   {
      printf ("OK\n");
   } else {
      printf ("ERROR\n");
   }

   /* Wait for completion */
   printf ("Flush ... ");
   result = fflush (fd);
   if (MQX_OK == result)
   {
      printf ("OK\n");
   } else {
      printf ("ERROR\n");
   }

   /* Restart I2C transfer for reading */
   printf ("Initiate repeated start ... ");
   if (I2C_OK == ioctl (fd, IO_IOCTL_I2C_REPEATED_START, NULL))
   {
      printf ("OK\n");
   } else {
      printf ("ERROR\n");
   }

   /* Set read request */
   param = n;
   printf ("Set number of bytes requested to %d ... ", param);
   if (I2C_OK == ioctl (fd, IO_IOCTL_I2C_SET_RX_REQUEST, &param))
   {
      printf ("OK\n");
   } else {
      printf ("ERROR\n");
   }

   /* Read all data */
   printf ("Read %d bytes ... ", n);
   result = fread (buffer, 1, n, fd);
   if (result == n)
   {
      printf ("OK\n");
   } else {
      printf ("ERROR\n");
   }

   /* Wait for completion */
   printf ("Flush ... ");
   result = fflush (fd);
   if (MQX_OK == result)
   {
      printf ("OK\n");
   } else {
      printf ("ERROR\n");
   }
      
   /* Stop I2C transfer - initiate EEPROM write cycle */
   printf ("Stop transfer ... ");
   if (I2C_OK == ioctl (fd, IO_IOCTL_I2C_STOP, NULL))
   {
      printf ("OK\n");
   } else {
      printf ("ERROR\n");
   }
} /* Endbody */


uint_8 asciitobin( uint_8 ascii )
{
	switch( ascii )
	{
		case '0': return(0);
		case '1': return(1);
		case '2': return(2);
		case '3': return(3);
		case '4': return(4);
		case '5': return(5);
		case '6': return(6);
		case '7': return(7);
		case '8': return(8);
		case '9': return(9);
		case 'a': return(10);
		case 'b': return(11);
		case 'c': return(12);
		case 'd': return(13);
		case 'e': return(14);
		case 'f': return(15);
		case 'A': return(10);
		case 'B': return(11);
		case 'C': return(12);
		case 'D': return(13);
		case 'E': return(14);
		case 'F': return(15);
		default:  return( 16 );
	}
}

uint_16 asciitobyte( uint_8 *nibble )
{
	uint_8 temph, templ;
	
	temph = asciitobin( *nibble );
	if( temph > 15 ) return( 0x100 );
	++nibble;
	templ = asciitobin( *nibble ); 
	if( templ > 15 ) return( 0x100 );
	++nibble;
	
	return( (temph<<4) + templ );
}

uint_8 NibbleToAscii( uint_8 nibble )
{
	switch( nibble )
	{
		case 0: return( '0' ); break;
		case 1: return( '1' ); break;
		case 2: return( '2' ); break;
		case 3: return( '3' ); break;
		case 4: return( '4' ); break;
		case 5: return( '5' ); break;
		case 6: return( '6' ); break;
		case 7: return( '7' ); break;
		case 8: return( '8' ); break;
		case 9: return( '9' ); break;
		case 10: return( 'A' ); break;
		case 11: return( 'B' ); break;
		case 12: return( 'C' ); break;
		case 13: return( 'D' ); break;
		case 14: return( 'E' ); break;
		case 15: return( 'F' ); break;		
	}
	
	return( '!' );
}


void ByteToAscii( uint_8 bin, uint_8 index )
{
	g_curr_send_buf[index] = NibbleToAscii(bin>>4);	
	g_curr_send_buf[index+1] = NibbleToAscii(bin&0x0F);	
}

void ReturnResponse( uint_8 size )
{
	uint_8  out_index;
	uint_8  checksum;
	
	checksum = 0;
	g_curr_send_buf[0] = '{';
	
	/* Copy Buffer to Send Buff */
	for (out_index = 0; out_index < size; out_index++)
	{
		ByteToAscii( g_ReturnData[out_index], ((out_index*2)+1) );
		checksum += g_ReturnData[out_index];	
	}
	
	ByteToAscii( (uint_8)(0x100-checksum), ((out_index*2)+1) );
	g_curr_send_buf[((out_index*2)+3)] = '}';
	g_send_size = size*2+4; 
}

void PositiveResponse( void )
{
		g_curr_send_buf[0] = '{';
		g_curr_send_buf[1] = '+';
		g_curr_send_buf[2] = '}';
		g_send_size = 3;
}

void ProcessCommand( uint_8 *astring )
{
	uint_16 temp;
	uint_32 param;
	uint_16 *pwm;
	uint_8  Error;
	uint_8  CheckSum;
	uint_8  *CalcCheck;
	uint_8	parms[4];
	
	Error = 1;
	if( astring[0] != '<' ) return;
	
	CalcCheck = astring+1;
	for( CheckSum=0; (temp = asciitobyte( CalcCheck ))<0x100; CalcCheck+=2 )
		CheckSum += (uint_8)temp;
		
	if( CheckSum )
	{
		g_curr_send_buf[0] = '{';
		g_curr_send_buf[1] = '~';
		g_curr_send_buf[2] = '}';
		g_send_size = 3;
		return;
	}
		
	switch( asciitobyte(++astring) )
	{
		case 0: // Read version and sync
	    	g_curr_send_buf[0] = '^';
    		g_curr_send_buf[1] = 'E';
    		g_curr_send_buf[2] = 'M';
    		g_curr_send_buf[3] = 'G';
    		g_curr_send_buf[4] = '0';
    		g_curr_send_buf[5] = '0';
    		g_curr_send_buf[6] = '2';
    		g_curr_send_buf[7] = ';';
			g_send_size = 8;    
			Error = 0;		
			break;			

		// 1250 + byte*20
		case 1: // GPT0 = J31
			astring += 2;
			temp = asciitobyte(astring);
			if( temp < 0x100 )
			{
			   if (I2C_OK == ioctl (g_fd, IO_IOCTL_I2C_ERICS_HACK0, &param))
			   {
			   		pwm = (uint_16*)param;
				    *pwm = 1250 + temp*20;
				    Error = 0;
				    PositiveResponse();
			   }
			}
			break;			

		case 2: // GPT1 = J33
			astring += 2;
			temp = asciitobyte(astring);
			if( temp < 0x100 )
			{
			   if (I2C_OK == ioctl (g_fd, IO_IOCTL_I2C_ERICS_HACK1, &param))
			   {
			   		pwm = (uint_16*)param;
				    *pwm = 1250 + temp*20;
				    Error = 0;
				    PositiveResponse();
			   }
			}
			break;			

		case 3: // GPT2 = J35
			astring += 2;
			temp = asciitobyte(astring);
			if( temp < 0x100 )
			{
			   if (I2C_OK == ioctl (g_fd, IO_IOCTL_I2C_ERICS_HACK2, &param))
			   {
			   		pwm = (uint_16*)param;
				    *pwm = 1250 + temp*20;
				    Error = 0;
				    PositiveResponse();
			   }
			}
			break;	
			
		case 4: // GPT3 = J37
			astring += 2;
			temp = asciitobyte(astring);
			if( temp < 0x100 )
			{
			   if (I2C_OK == ioctl (g_fd, IO_IOCTL_I2C_ERICS_HACK3, &param))
			   {
			   		pwm = (uint_16*)param;
				    *pwm = 1250 + temp*20;
				    Error = 0;
				    PositiveResponse();
			   }
			}
			break;			
		
		case 9: // GPT0 = J31, GPT1 = J33,  GPT2 = J35,  GPT3 = J37
			astring += 2;
			temp = asciitobyte(astring);
			if( temp < 0x100 )
			{
			   if (I2C_OK == ioctl (g_fd, IO_IOCTL_I2C_ERICS_HACK0, &param))
			   {
			   		pwm = (uint_16*)param;
				    *pwm = 1250 + temp*20;
				    Error = 0;
			   }
			}
			else
			{
				Error = 1;
				break;
			}
			
			astring += 2;
			temp = asciitobyte(astring);
			if( temp < 0x100 )
			{
			   if (I2C_OK == ioctl (g_fd, IO_IOCTL_I2C_ERICS_HACK1, &param))
			   {
			   		pwm = (uint_16*)param;
				    *pwm = 1250 + temp*20;
				    Error = 0;
			   }
			}
			else
			{
				Error = 1;
				break;
			}

			astring += 2;
			temp = asciitobyte(astring);
			if( temp < 0x100 )
			{
			   if (I2C_OK == ioctl (g_fd, IO_IOCTL_I2C_ERICS_HACK2, &param))
			   {
			   		pwm = (uint_16*)param;
				    *pwm = 1250 + temp*20;
				    Error = 0;
			   }
			}
			else
			{
				Error = 1;
				break;
			}

			astring += 2;
			temp = asciitobyte(astring);
			if( temp < 0x100 )
			{
			   if (I2C_OK == ioctl (g_fd, IO_IOCTL_I2C_ERICS_HACK3, &param))
			   {
			   		pwm = (uint_16*)param;
				    *pwm = 1250 + temp*20;
				    Error = 0;
			   }
			}
			else
			{
				Error = 1;
				break;
			}

			PositiveResponse();
			break;	
			
		case 10: // accelread
#if MMA7455	
	// MMA7455
#if MMA7455_10BIT
			i2c_read_register( g_fd, 0x1D, 0x00,g_ReturnData, 6 );
			ReturnResponse(6); // returns 6 bytes			
#else
			i2c_read_register( g_fd, 0x1D, 0x06,g_ReturnData, 3 );
			ReturnResponse(3); // returns 3 bytes
#endif
#endif			
			Error = 0;
			break;
			
		case 11: // iicwrite
			Error = 0;
			for( temp = 0; temp<3; temp++ )
			{
				astring += 2;
				parms[temp] = asciitobyte(astring);
				if( parms[temp] > 0xff ) 
				{
					Error = 1;
					break;
				}
				
				if( Error ) break;
			}
			
			i2c_write_register( g_fd, parms[0], parms[1], &parms[2], 1 );
			PositiveResponse();
			Error = 0;
			break;

		case 12: // iicread
			Error = 0;
			for( temp = 0; temp<3; temp++ )
			{
				astring += 2;
				parms[temp] = asciitobyte(astring);
				if( parms[temp] > 0xff ) 
				{
					Error = 1;
					break;
				}
				
				if( Error ) break;
			}
			
			i2c_read_register( g_fd, parms[0], parms[1], g_ReturnData, parms[2] );
			ReturnResponse( parms[2] );
			Error = 0;
			break;

		case 13: // LED
			Error = 0;
			astring += 2;
			parms[0] = asciitobyte(astring);
			if( parms[0] > 0xff ) 
			{
				Error = 1;
				break;
			}
				
			if( Error ) break;
			
			// Set to output
			i2c_write_byte( g_fd, 0x38, 0x03, 0x00 );
			_time_delay (5L);
			i2c_write_register( g_fd, 0x38, 0x01, &parms[0], 1 );
			PositiveResponse();
			Error = 0;
			break;

		case 14: // GPIOINIT
			Error = 0;
			astring += 2;
			parms[0] = asciitobyte(astring);
			if( parms[0] > 0xff ) 
			{
				Error = 1;
				break;
			}
				
			if( Error ) break;
			
			// Set to output
			i2c_write_register( g_fd, 0x38, 0x03, &parms[0], 1 );
			PositiveResponse();
			Error = 0;
			break;

		case 15: // GPIOOUT
			Error = 0;
			astring += 2;
			parms[0] = asciitobyte(astring);
			if( parms[0] > 0xff ) 
			{
				Error = 1;
				break;
			}
				
			if( Error ) break;
			
			// Set to output
			i2c_write_register( g_fd, 0x38, 0x01, &parms[0], 1 );
			PositiveResponse();
			Error = 0;
			break;

		case 16: // GPIOIN			
			i2c_read_register( g_fd, 0x38, 0, g_ReturnData, 1 );
			ReturnResponse( 1 );
			Error = 0;
			break;

		case 17: // ADREAD
		{
			ADC_RESULT_STRUCT data;
		
			Error = 0;
			astring += 2;
			parms[0] = asciitobyte(astring);
			if( parms[0] > 0xff ) 
			{
				Error = 1;
				break;
			}
				
			if( Error ) break;
			
			switch( parms[0] )
			{
				default: Error=1; break;
				case 0:
        			(void)read(g_fdadc0, &data, sizeof(data) );
        			break;
			}

			if( Error ) break;
        	g_ReturnData[0] = data.result;
			ReturnResponse( 1 );
			Error = 0;
			break;
		}

		case 18: // touchread
		{
			i2c_read_register( g_fd, I2C_ID, 0, g_ReturnData, 2 );
			ReturnResponse( 2 );
			Error = 0;
			break;
		}		
	}

	if( Error )
	{
		g_curr_send_buf[0] = '{';
		g_curr_send_buf[1] = '!';
		g_curr_send_buf[2] = '}';
		g_send_size = 3;
	}
}


void InitTouch( void )
{
	INIT_MPR121(0x2B, 0x01) //AN3891 A
	INIT_MPR121(0x2C, 0x01) //AN3891 A
	INIT_MPR121(0x2D, 0x00) //AN3891 A
	INIT_MPR121(0x2E, 0x00) //AN3891 A
	INIT_MPR121(0x2F, 0x01) //AN3891 B
	INIT_MPR121(0x30, 0x01) //AN3891 B
	INIT_MPR121(0x31, 0xFF) //AN3891 B
	INIT_MPR121(0x32, 0x02) //AN3891 B
	INIT_MPR121(0x41, 0x0F) //AN3892 C
	INIT_MPR121(0x42, 0x0A) //AN3892 C
	INIT_MPR121(0x43, 0x0F) //AN3892 C
	INIT_MPR121(0x44, 0x0A) //AN3892 C
	INIT_MPR121(0x45, 0x0F) //AN3892 C
	INIT_MPR121(0x46, 0x0A) //AN3892 C
	INIT_MPR121(0x47, 0x0F) //AN3892 C
	INIT_MPR121(0x48, 0x0A) //AN3892 C
	INIT_MPR121(0x49, 0x0F) //AN3892 C
	INIT_MPR121(0x4A, 0x0A) //AN3892 C
	INIT_MPR121(0x4B, 0x0F) //AN3892 C
	INIT_MPR121(0x4C, 0x0A) //AN3892 C
	INIT_MPR121(0x4D, 0x0F) //AN3892 C
	INIT_MPR121(0x4E, 0x0A) //AN3892 C
	INIT_MPR121(0x4F, 0x0F) //AN3892 C
	INIT_MPR121(0x50, 0x0A) //AN3892 C
	INIT_MPR121(0x51, 0x0F) //AN3892 C
	INIT_MPR121(0x52, 0x0A) //AN3892 C
	INIT_MPR121(0x53, 0x0F) //AN3892 C
	INIT_MPR121(0x54, 0x0A) //AN3892 C
	INIT_MPR121(0x55, 0x0F) //AN3892 C
	INIT_MPR121(0x56, 0x0A) //AN3892 C
	INIT_MPR121(0x57, 0x0F) //AN3892 C
	INIT_MPR121(0x58, 0x0A) //AN3892 C
	INIT_MPR121(0x5D, 0x04) //AN3890 D
	INIT_MPR121(0x7B, 0x0B) //AN3889 F
	INIT_MPR121(0x7D, 0x9C) //AN3889 F
	INIT_MPR121(0x7E, 0x65) //AN3889 F
	INIT_MPR121(0x7F, 0x8C) //AN3889 F
	INIT_MPR121(0x5E, 0x0C) //AN3890 E	
}

void InitAD( void )
{
	FILE_PTR  f; 

    f = fopen(MY_ADC, (const char*)&adc_init);
    if(f == NULL)
    {    
      	printf ("Failed to open ADC!\n");
      	_time_delay (200L);
      	_mqx_exit (1L);
    }

    g_fdadc0 = fopen(MY_ADC "first", (const char*)&adc_channel_param1);
    if(g_fdadc0 == NULL)
    {    
      	printf ("Failed to open ADC channel 0!\n");
      	_time_delay (200L);
      	_mqx_exit (1L);
    }	
}


void RobotInit( void )
{
    uint_16		*pwm;
    uint_32 	param;


   /* Open the I2C driver */         
   g_fd = fopen ("i2c0:", NULL);   
   if (g_fd == NULL) 
   {
      printf ("Failed to open the I2C driver!\n");
      _time_delay (200L);
      _mqx_exit (1L);
   }

   if (I2C_OK != ioctl (g_fd, IO_IOCTL_I2C_SET_MASTER_MODE, NULL))
   {
      printf ("Failed to set I2C to Master Mode!\n");
      _time_delay (200L);
      _mqx_exit (1L);
   }

	if (I2C_OK == ioctl (g_fd, IO_IOCTL_I2C_ERICS_HACK0, &param))
	{
		pwm = (uint_16*)param;
		*pwm = 1250 + 0x80*20;
	}
	if (I2C_OK == ioctl (g_fd, IO_IOCTL_I2C_ERICS_HACK1, &param))
	{
		pwm = (uint_16*)param;
		*pwm = 1250 + 0x80*20;
	}
	if (I2C_OK == ioctl (g_fd, IO_IOCTL_I2C_ERICS_HACK2, &param))
	{
		pwm = (uint_16*)param;
		*pwm = 1250 + 0x80*20;
	}
	if (I2C_OK == ioctl (g_fd, IO_IOCTL_I2C_ERICS_HACK3, &param))
	{
		pwm = (uint_16*)param;
		*pwm = 1250 + 0x80*20;
	}

#if MMA7455	
	// MMA7455
#if MMA7455_10BIT
	g_packet[0] = 0x01;		// 8G, measurement mode, read 10bit registers
#else
    g_packet[0] = 0x05;		// 2G, measurement mode, must read 8bit registers
#endif
    i2c_write_register( g_fd, 0x1D, 0x16, g_packet, 1 );
#endif   

	InitAD();
	InitTouch();
}

// END ROBOT ------------------------------------------------------------------------------------------------
// END ROBOT ------------------------------------------------------------------------------------------------
// END ROBOT ------------------------------------------------------------------------------------------------
// END ROBOT ------------------------------------------------------------------------------------------------


// START SERIAL -----------------------------------------------------------------------------------------------------------
// START SERIAL -----------------------------------------------------------------------------------------------------------
// START SERIAL -----------------------------------------------------------------------------------------------------------
// START SERIAL -----------------------------------------------------------------------------------------------------------
// START SERIAL -----------------------------------------------------------------------------------------------------------

void SERIALRobot(void)
{
    uint_8 data;
	uint_8 size;

    g_curr_recv_buf = USB_mem_alloc_zero(DATA_BUFF_SIZE);
    g_curr_send_buf = USB_mem_alloc_zero(DATA_BUFF_SIZE);
	packet_in_queue_in_index  = 1;
	packet_in_queue_out_index = 0;
    g_packet = USB_mem_alloc_zero(DATA_BUFF_SIZE);
    g_ReturnData = USB_mem_alloc_zero(DATA_BUFF_SIZE);
    g_packet_index = 0;
    g_boot_message = 1;

    _int_disable();  // WHY DO INTERRUPTS NEED TO BE DISABLED????  
        
	RobotInit();

    _int_enable();
    

	printf( "^EMG001;" );
	
	while( 1 )
	{
		data = fgetc(stdin);
    	if( g_packet_index == 0 )
    	{
    		// wait for <
    		if( data != '<' )
    			continue;
    	}
	        
	    if( g_packet_index < (DATA_BUFF_SIZE-1 ))
	  	 	g_packet[g_packet_index++] = data;
	        
	    if( data == '>' )
	    {	        	
	        g_packet_index = 0;
	        ProcessCommand( g_packet );
	    }
	    		
    
		if(g_send_size) 
		{
			size = g_send_size;
		    g_send_size = 0;
		    fwrite( g_curr_send_buf,size, 1, stdout );
		}
	} // while
    return;
}



// START USB -----------------------------------------------------------------------------------------------------------
// START USB -----------------------------------------------------------------------------------------------------------
// START USB -----------------------------------------------------------------------------------------------------------
// START USB -----------------------------------------------------------------------------------------------------------
// START USB -----------------------------------------------------------------------------------------------------------
 /*****************************************************************************
 *  
 *   @name        USBRobot
 * 
 *   @brief       This function is the entry for mouse (or other usuage)
 * 
 *   @param       None
 * 
 *   @return      None
 **                
 *****************************************************************************/
void USBRobot(void)
{       
    //uint_8   error;
    CDC_CONFIG_STRUCT cdc_config;
    USB_CLASS_CDC_ENDPOINT * endPoint_ptr;

    g_curr_recv_buf = USB_mem_alloc_zero(DATA_BUFF_SIZE);
    g_curr_send_buf = USB_mem_alloc_zero(DATA_BUFF_SIZE);
	packet_in_queue_in_index  = 1;
	packet_in_queue_out_index = 0;
    g_packet = USB_mem_alloc_zero(DATA_BUFF_SIZE);
    g_ReturnData = USB_mem_alloc_zero(DATA_BUFF_SIZE);
    g_packet_index = 0;
    g_boot_message = 1;
    
        
    endPoint_ptr = USB_mem_alloc_zero(sizeof(USB_CLASS_CDC_ENDPOINT)*CDC_DESC_ENDPOINT_COUNT);
    cdc_config.comm_feature_data_size = COMM_FEATURE_DATA_SIZE;
    cdc_config.usb_ep_data = &usb_desc_ep;
    cdc_config.desc_endpoint_cnt = CDC_DESC_ENDPOINT_COUNT;
    cdc_config.cdc_class_cb.callback = USB_App_Callback;
    cdc_config.cdc_class_cb.arg = &g_app_handle;
    cdc_config.vendor_req_callback.callback = NULL;
    cdc_config.vendor_req_callback.arg = NULL;
    cdc_config.param_callback.callback = USB_Notif_Callback;
    cdc_config.param_callback.arg = &g_app_handle;
    cdc_config.desc_callback_ptr =  &desc_callback;
    cdc_config.ep = endPoint_ptr;
    cdc_config.cic_send_endpoint = CIC_NOTIF_ENDPOINT;
    /* Always happend in control endpoint hence hard coded in Class layer*/
    //cdc_config.cic_recv_endpoint = 
    cdc_config.dic_send_endpoint = DIC_BULK_IN_ENDPOINT;
    cdc_config.dic_recv_endpoint = DIC_BULK_OUT_ENDPOINT;
    
    _int_disable();    
    
    /* Initialize the USB interface */
    g_app_handle = USB_Class_CDC_Init(&cdc_config);
 	g_recv_size = 0;
    g_send_size= 0;   
    
	RobotInit();

    _int_enable();
  
    while (TRUE) 
    {
        /* call the periodic task function */      
        USB_CDC_Periodic_Task();           

       /*check whether enumeration is complete or not */
        if((start_app==TRUE) && (start_transactions==TRUE))
        {        
            Virtual_Com_App(); 
        }            
    }/* Endwhile */   
} 


/******************************************************************************
 * 
 *    @name       Virtual_Com_App
 *    
 *    @brief      
 *                  
 *    @param      None
 * 
 *    @return     None
 *    
 *****************************************************************************/
void Virtual_Com_App(void)
{
    uint_8 error;
    uint_8 data;
	uint_8 size;
    
    if( g_boot_message )
    {
    	g_boot_message = 0;
    	g_curr_send_buf[0] = '^';
    	g_curr_send_buf[1] = 'E';
    	g_curr_send_buf[2] = 'M';
    	g_curr_send_buf[3] = 'G';
    	g_curr_send_buf[4] = '0';
    	g_curr_send_buf[5] = '0';
    	g_curr_send_buf[6] = '2';
    	g_curr_send_buf[7] = ';';
    	
		size = 8;
		USB_Class_CDC_Send_Data(g_app_handle,DIC_BULK_IN_ENDPOINT,
		     	g_curr_send_buf,size);
		if(error != USB_OK) 
		{
		            /* Failure to send Data Handling code here */
		} 
    }
    
    /* User Code */ 
	    while( (uint_8)(packet_in_queue_out_index+1) != packet_in_queue_in_index )
	    {
	    	data = packet_in_queue[++packet_in_queue_out_index];
    		if( g_packet_index == 0 )
    		{
    			// wait for <
    			if( data != '<' )
    				continue;
    		}
	        
	        if( g_packet_index < (DATA_BUFF_SIZE-1 ))
	        	g_packet[g_packet_index++] = data;
	        
	        if( data == '>' )
	        {	        	
#if 0
		        /* Copy Buffer to Send Buff */
		        for (error = 0; error < g_packet_index; error++)
		        {
		        	g_curr_send_buf[error] = g_packet[error];	
		        }
		        g_send_size = g_packet_index; 
#endif
		        g_packet_index = 0;
		        ProcessCommand( g_packet );
	        }
	    		
#if 0
        /* Copy Buffer to Send Buff */
        for (error = 0; error < g_recv_size; error++)
        {
        	g_curr_send_buf[error] = g_curr_recv_buf[error];	
        }
        g_send_size = g_recv_size; 
#endif
    
		    if(g_send_size) 
		    {
		        size = g_send_size;
		        g_send_size = 0;
		     USB_Class_CDC_Send_Data(g_app_handle,DIC_BULK_IN_ENDPOINT,
		     	g_curr_send_buf,size);
		        if(error != USB_OK) 
		        {
		            /* Failure to send Data Handling code here */
		        } 
		    }
	    } // while
    return;
}

/******************************************************************************
 * 
 *    @name        USB_App_Callback
 *    
 *    @brief       This function handles the callback  
 *                  
 *    @param       handle : handle to Identify the controller
 *    @param       event_type : value of the event
 *    @param       val : gives the configuration value 
 * 
 *    @return      None
 *
 *****************************************************************************/
void USB_App_Callback(uint_8 event_type, void* val,pointer arg) 
{
    UNUSED_ARGUMENT (arg)
    UNUSED_ARGUMENT (val)    
    if(event_type == USB_APP_BUS_RESET) 
    {
        start_app=FALSE;    
    }
    else if(event_type == USB_APP_ENUM_COMPLETE) 
    {
        start_app=TRUE;        
    }
    else if(event_type == USB_APP_ERROR)
    {
    	/* add user code for error handling */
    }
    
    return;
}

/******************************************************************************
 * 
 *    @name        USB_Notif_Callback
 *    
 *    @brief       This function handles the callback  
 *                  
 *    @param       handle:  handle to Identify the controller
 *    @param       event_type : value of the event
 *    @param       val : gives the configuration value 
 * 
 *    @return      None
 *
 *****************************************************************************/
 
void USB_Notif_Callback(uint_8 event_type,void* val,pointer arg) 
{
    uint_32 handle;
    uint_8 index;
    
    handle = *((uint_32 *)arg);
    if(start_app == TRUE) 
    {
#if DTE_SUPPORT    
        if(event_type == USB_APP_CDC_DTE_ACTIVATED) 
        {
           start_transactions = TRUE;        
        } 
        else if(event_type == USB_APP_CDC_DTE_DEACTIVATED) 
        {
           start_transactions = FALSE;        
        }
        else if((event_type == USB_APP_DATA_RECEIVED)&&
                                              (start_transactions == TRUE))
#else
		if(event_type == USB_APP_DATA_RECEIVED)
#endif
        {
           
            uint_32 BytesToBeCopied;            
            APP_DATA_STRUCT* dp_rcv;
            dp_rcv = (APP_DATA_STRUCT*)val; 
            
#if !DTE_SUPPORT            
            start_transactions = TRUE;
#endif            
            
            BytesToBeCopied = dp_rcv->data_size;
            for(index = 0; index<BytesToBeCopied ; index++) 
            {
            	if( packet_in_queue_out_index == packet_in_queue_in_index )
            	{            		
            		// queue is full, drop chars
            		break; 
            	}
              //  g_curr_recv_buf[index]= dp_rcv->data_ptr[index];
              	packet_in_queue[packet_in_queue_in_index++] = dp_rcv->data_ptr[index];
            }
            g_recv_size = index;
            USB_Class_CDC_Recv_Data(handle,DIC_BULK_OUT_ENDPOINT,
          		g_curr_recv_buf,DIC_BULK_OUT_ENDP_PACKET_SIZE);	
        }        
        else if((event_type == USB_APP_SEND_COMPLETE)&&
        	(start_transactions == TRUE))
        { 
            /* Previous Send is complete. Queue next recieve */ 
//            USB_Class_CDC_Recv_Data(handle,DIC_BULK_OUT_ENDPOINT,
//          		g_curr_recv_buf,DIC_BULK_OUT_ENDP_PACKET_SIZE);	
        }
    }
    return;
}
// END USB -----------------------------------------------------------------------------------------------------------
// END USB -----------------------------------------------------------------------------------------------------------
// END USB -----------------------------------------------------------------------------------------------------------
// END USB -----------------------------------------------------------------------------------------------------------
// END USB -----------------------------------------------------------------------------------------------------------


/*FUNCTION*----------------------------------------------------------------
* 
* Function Name  : Main_Task
* Returned Value : None
* Comments       :
*     First function called.  Calls the Test_App
*     callback functions.
* 
*END*--------------------------------------------------------------------*/
void Main_Task
   (
      uint_32 param
   )
{   
	UNUSED_ARGUMENT (param)
#if ROBOT_SERIAL_MODE	
	SERIALRobot();
#else
    USBRobot();
#endif  
}

/* EOF */